#include "geometry.hpp"
namespace chen {

Point2::Point2 (): x(0.0), y(0.0){assert(!isNan(*this));}
Point2::Point2 (const Float & f): x(f), y(f) {assert(!isNan(*this));}
Point2::Point2 (Float a, Float b): x(a), y(b) {assert(!isNan(*this));}
Point2::Point2 (const Vector2 & v): x(v.x), y(v.y) {assert(!isNan(*this));}
Point2::Point2 (const Point2 & p): x(p.x), y(p.y) {assert(!isNan(*this));}
Point2::operator Vector2() const {assert(!isNan(*this)); return Vector2(x, y);}
Point2 & Point2::operator = (const Point2 & b) {x = b.x, y = b.y; assert(!isNan(*this)); return * this;}
Point2 Point2::operator + (const Vector2 & b) const {assert(!isNan(*this) && !isNan(b)); return Point2(x+b.x, y+b.y);}
Point2 Point2::operator - (const Vector2 & b) const {assert(!isNan(*this) && !isNan(b)); return Point2(x-b.x, y-b.y);}
Point2 Point2::operator * (const Float & b) const {assert(!isNan(*this) && !isNan(b)); return Point2(x*b, y*b);}
Point2 Point2::operator / (const Float & b) const {assert(!isNan(*this) && !isNan(b)); return Point2(x/b, y/b);}
Point2 Point2::operator *= (const Float & b) {x *= b, y *= b; assert(!isNan(*this)); return *this;}
Point2 Point2::operator - () const {assert(!isNan(*this)); return Point2(-x, -y);}
Float Point2::lengthSquared () const {return x*x+y*y;}
Point2 Point2::normalized () const {assert(lengthSquared() != 0); return *this/std::sqrt(x*x+y*y);}
void Point2::resize (const Float & f) {assert(lengthSquared() != 0); (*this) *= (f/std::sqrt(this->lengthSquared())); }
Point2 Point2::resized (const Float & f) const {assert(lengthSquared() != 0); return (*this)*(f/std::sqrt(this->lengthSquared()));}



Point3 offsetOrigin (const Point3 & o, const Vector3 & err, const Normal & n) {
    Vector3 of = (Vector3)((Vector3)n*dot(abs((Vector3)n), err));
    return o+of; 
}




Matrix4::Matrix4 (const Float**from) {memcpy(m, from, sizeof m);}
Matrix4::Matrix4 (const Matrix4 & b) {memcpy(m, b.m, sizeof m);}
Matrix4 & Matrix4::operator = (const Matrix4 & b) {memcpy(m, b.m, sizeof m); return * this;}
const Float* Matrix4::operator [] (int i) const {return m[i];}
Float* Matrix4::operator [] (int i) {return m[i];}
Matrix4 Matrix4::operator * (const Matrix4 & b) const {
    Matrix4 ret;
    for(int i = 0; i<4; i++)
        for(int j = 0; j<4; j++)
            ret[i][j] = m[i][0]*b[0][j]+m[i][1]*b[1][j]+m[i][2]*b[2][j]+m[i][3]*b[3][j];
    return ret;
}




bool inverse (const Matrix4 & _m, Matrix4 & _mr) {
    long double mr[4][4];
    memset(mr, 0, sizeof mr);
    mr[0][0] = mr[1][1] = mr[2][2] = mr[3][3] = 1.0;
    long double m[4][4];
    for(int i = 0; i<4; i++)
        for(int j = 0; j<4; j++)
            m[i][j] = _m[i][j];
    for(int i = 0; i<4; i++) {
        int p = -1;
        for(int j = 0; j<4; j++)
            if(m[j][i] != 0.0f)
                {p = j; break ;}
        if(p == -1) return false;
        for(int j = 0; j<4; j++)
            if(j != p && m[j][i] != 0.0) {
                Float frc = m[j][i]/m[p][i];
                for(int k = 0; k<4; k++)
                    m[j][k] -= m[p][k]*frc,
                    mr[j][k] -= mr[p][k]*frc;
            }
        if(p != i) {
            Float t;
            for(int j = 0; j<4; j++) {
                t = m[i][j], m[i][j] = m[p][j], m[p][j] = t;
                t = mr[i][j], mr[i][j] = mr[p][j], mr[p][j] = t;
            }
        }
    }
    for(int i = 0; i<4; i++)
        if(!eequal(m[i][i], 1.0f)) {
            Float frc = 1/m[i][i];
            for(int j = 0; j<4; j++)
                mr[i][j] *= frc;
        }
    for(int i = 0; i<4; i++)
        for(int j = 0; j<4; j++)
            _mr[i][j] = mr[i][j];
    return true;
}



Bound2::Bound2 ():  mn(0, 0), mx(0, 0) {}
Bound2::Bound2 (const Point2 & v): mn(v), mx(v) {}
Bound2::Bound2 (const Point2 & a, const Point2 & b) {
    mx = Point2(max(a.x, b.x), max(a.y, b.y));
    mn = Point2(min(a.x, b.x), min(a.y, b.y));
}

Bound3::Bound3 (): mn(0, 0, 0), mx(0, 0, 0)  {}
Bound3::Bound3 (const Point3 & v): mn(v), mx(v) {}
Bound3::Bound3 (const Point3 & a, const Point3 & b) {
    mx = Point3(max(a.x, b.x), max(a.y, b.y), max(a.z, b.z));
    mn = Point3(min(a.x, b.x), min(a.y, b.y), min(a.z, b.z));
}
bool Bound3::bound (const Point3 & p) const {
    return mn.x < p.x && p.x < mx.x && mn.y < p.y && p.y < mx.y && mn.z < p.z && p.z < mx.z;
}

Bound2 combine(const Bound2 & a, const Bound2 & b) {
    return Bound2(
        Point2(min(a.mn.x, b.mn.x), min(a.mn.y, b.mn.y)),
        Point2(max(a.mx.x, b.mx.x), max(a.mx.y, b.mx.y))
    );
}

Bound3 combine(const Bound3 & a, const Bound3 & b) {
    return Bound3(
        Point3(min(a.mn.x, b.mn.x), min(a.mn.y, b.mn.y), min(a.mn.z, b.mn.z)),
        Point3(max(a.mx.x, b.mx.x), max(a.mx.y, b.mx.y), max(a.mx.z, b.mx.z))
    );
}

Transform scale (Vector3 s) {
    Transform ret;
    ret.m[0][0] = s.x;
    ret.m[1][1] = s.y;
    ret.m[2][2] = s.z;
    ret.m[3][3] = 1.0f;
    ret.minv[0][0] = 1.0/s.x;
    ret.minv[1][1] = 1.0/s.y;
    ret.minv[2][2] = 1.0/s.z;
    ret.minv[3][3] = 1.0f;
    return ret;
}
Transform shift (Vector3 v) {
    Transform ret;
    ret.m[0][0] = ret.m[1][1] = ret.m[2][2] = ret.m[3][3] = 1.0f;
    ret.m[0][3] = v.x, ret.m[1][3] = v.y, ret.m[2][3] = v.z;
    ret.minv[0][0] = ret.minv[1][1] = ret.minv[2][2] = ret.minv[3][3] = 1.0f;
    ret.minv[0][3] = -v.x, ret.minv[1][3] = -v.y, ret.minv[2][3] = -v.z;
    return ret;
}

Transform rotateX (Float v) {
    Matrix4 t(1);
    t.m[1][1] = cos(v);
    t.m[1][2] = -sin(v);
    t.m[2][1] = sin(v);
    t.m[2][2] = cos(v);
    return Transform(t);
}


Transform rotateY (Float v) {
    Matrix4 t(1);
    t.m[0][0] = cos(v);
    t.m[0][2] = sin(v);
    t.m[2][0] = -sin(v);
    t.m[2][2] = cos(v);
    return Transform(t);
}

Transform rotateZ (Float v) {
    Matrix4 t(1);
    t.m[0][0] = cos(v);
    t.m[0][1] = -sin(v);
    t.m[1][0] = sin(v);
    t.m[1][1] = cos(v);
    return Transform(t);
}

Transform transform_unit(Matrix4(1.0));

}
